#!/usr/bin/env python
# -*- coding:utf-8 -*-
# @FileName  :loop.py
# @Time      :2024/9/8 3:59
# @Author    :YKW
import numpy as np
from scipy.optimize import fsolve
import random
import matplotlib.pyplot as plt
from shapely.geometry import Point, LineString


class cycle:
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.r = None

    def cal_distance(self, x, y):
        """
        其他点到圆心距离
        """
        return np.sqrt((x - self.x) ** 2 + (y - self.y) ** 2)

    def fun_find(self, x, y, r):
        return self.cal_distance(x, y) - r


def dt(a, b):
    """
    向量点积
    """
    return a[0] * b[0] + a[1] * b[1]


class loop:
    """
        模拟或计算涉及循环路径的物理系统
        通过笛卡尔坐标和极坐标之间的转换，以及相关的几何计算，来模拟和处理循环路径上点的位置和运动。
    """

    def __init__(self, a=0, b=1.7, tol=0.01):
        self.a = a
        self.b = b  # 螺距
        self.r = None  # 极坐标半径
        self.theta = None  # 极坐标极角
        self.x = None  # 笛卡尔坐标系 x
        self.y = None  # 笛卡尔坐标系 y
        self.state = 0  # 0:no change 1:x,y 2:r,theta
        self.tol = tol  # 容忍度
        self.vec_xy = None  # 笛卡尔坐标系 到原点 倾斜角
        self.vec_rc = None  # None
        self.rc = None
        # self.n_r=None
        self.n_x = None  # 法线向量
        self.dot = None  # Point类，几何位置
        self.t_x = None  # 切线向量
        self.t_r = None  # None
        self.t_n = None  # None
        self.alpha = None

    def refresh(self):
        """
        根据state,更新参数
        """
        if self.state == 0:
            # print("NO change took place")
            pass
        if self.state == 2:
            self.x = self.r * np.cos(self.theta)
            self.y = self.r * np.sin(self.theta)
        elif self.state == 1:
            self.r = np.sqrt(self.x ** 2 + self.y ** 2)
            self.theta = self.r / self.b
        # self.n_r = (-np.sin(self.theta), np.cos(self.theta))
        self.n_x = np.array(((np.sin(self.theta) + self.theta * np.cos(self.theta)) / (np.cos(self.theta) -
                                                                                       self.theta * np.sin(self.theta)),
                             -1))
        # l=np.sqrt(self.n_r[0]**2+self.n_r[1]**2)
        # self.n_r=(self.n_r[0]/l,self.n_r[1]/l)
        l = np.sqrt(self.n_x[0] ** 2 + self.n_x[1] ** 2)
        self.n_x = np.array((self.n_x[0] / l, self.n_x[1] / l))  # 标准化 单位向量
        self.state = 0
        self.dot = Point(self.x, self.y)
        test_dot = Point(self.x + self.n_x[0], self.y + self.n_x[1])
        O = Point(0, 0)
        if test_dot.distance(O) > self.dot.distance(O):
            self.n_x = np.array((-self.n_x[0], -self.n_x[1]))
        self.t_x = np.array((self.n_x[1], -self.n_x[0]))
        if self.x * self.t_x[0] + self.y * self.t_x[1] < 0:  # it can proof that (x,y)*t_x is always positive
            self.t_x = np.array((-self.t_x[0], -self.t_x[1]))
        self.vec_xy = np.array((self.x, self.y))

    def get(self, m, n, state=1):
        """
        极坐标和笛卡尔坐标系互转
        """
        if state == 1:
            self.x = m
            self.y = n
            self.state = 1
            self.refresh()
        elif state == 2:

            self.theta = n
            self.r = self.b * n
            self.state = 2
            self.refresh()
        else:
            raise ValueError("state err")

    def check(self):
        """
        判断是否正确设置参数：1.极坐标和笛卡尔正确映射 2.r 和 b*theta相等
        """
        self.refresh()
        if abs(np.tan(self.theta) - self.y / self.x) > self.tol or abs(self.r - self.b * self.theta) > self.tol:
            raise ValueError("Not on loop")

    def cycle(self, rc):
        pass


class combined:
    """
    组合前后两个圆弧，方便计算
    """

    def __init__(self, d1, d2):
        d1.refresh()
        d2.refresh()
        self.dot1 = d1
        self.dot2 = d2

    # 返回掉头路径的长度
    def get(self, tol=0.001):  # experiment changing dot
        """
        只知道螺线和圆弧切点的时候，通过二分确定圆弧圆弧相切时的半径
        """
        # line0=LineString([self.dot1.dot,self.dot2.dot])
        rl = 0
        rr = 15  # min and max,change it if bug emerged
        while rl < rr:
            mid = (rl + rr) / 2
            r = mid
            # dot1是大圆 圆心坐标，要和self.dot1区分
            dot1 = Point(self.dot1.x + self.dot1.n_x[0] * r * 2, self.dot1.y + self.dot1.n_x[1] * r * 2)
            # line1=LineString([self.dot1.dot,tem])
            dot2 = Point(self.dot2.x + self.dot2.n_x[0] * r, self.dot2.y + self.dot2.n_x[1] * r)
            # line2=LineString([self.dot2.dot,tem])
            # dotc=line1.intersection(line2)
            # assert dotc.geo_type == "Point"
            # 满足圆弧段相切的条件
            if abs(dot1.distance(dot2) - 3 * r) <= tol:
                rl = rr = mid
                break
            if dot1.distance(dot2) - 3 * r > tol:
                rl = mid
            else:
                rr = mid

        self.dot1.rc = 2 * rl
        self.dot2.rc = rl

        vec = np.array((dot1.x - dot2.x, dot1.y - dot2.y))
        lvec = np.sqrt(vec[0] ** 2 + vec[1] ** 2)
        vec = (vec[0] / lvec, vec[1] / lvec)  # 单位化方向向量
        alpha1 = dt(vec, self.dot1.n_x)
        alpha2 = dt(vec, self.dot2.n_x)
        alpha1 = np.arccos(alpha1)  #
        alpha2 = np.arccos(alpha2)
        vec1 = -self.dot1.t_x
        if np.dot(self.dot1.t_x, vec) > 0:
            alpha1 = 2 * np.pi - alpha1
        if np.dot(self.dot2.t_x, vec) > 0:
            alpha2 = 2 * np.pi - alpha2

        self.dot1.alpha = alpha1  # 大圆圆心角
        self.dot2.alpha = alpha2  # 小圆圆心角

        return alpha1 * 2 * rl + alpha2 * rl  # alp1*rc1+alp2*rc2


def generate(mul, b=1.7 / (2 * np.pi)):
    """
    在等距螺线上随机生成一个极角，计算出对应点坐标
    """
    # mul = 15 * np.pi * 2 / 1.7  # 螺距计算的系数
    theta = random.random() * mul
    r = theta * b
    # print("mul:{%d}" % mul)
    while 2 * r < 10:
        print("change!")
        theta = random.random() * mul
        r = theta * b
    x = r * np.cos(theta)
    y = r * np.sin(theta)
    return np.array((x, y))


def generate_theta(mul):
    return random.random() * mul


# 目标函数
def objective_function(x):
    return x.get()


def check_able(com):
    """
    检测现在的曲线能否不碰撞
    """
    pass


# 产生新的解
def get_new_solution(current_solution):
    state = 1
    init_population = []
    a = 0.1
    mul = 4.5 * np.pi * 2 / 1.7 * a  # 55
    loop1 = loop()
    loop2 = loop()
    p1 = generate_theta(mul) * a + current_solution.dot1.theta  # random * mul * a + theta
    p2 = generate_theta(mul) * a + current_solution.dot2.theta
    loop1.get(0, p1, state=2)  # loop1的切点是p1
    loop2.get(0, p2, state=2)  # loop2的切点是p2
    com = combined(loop1, loop2)  # 满足相切约束,满足2倍圆弧约束
    # while check_able(com):
    while check_able(com):  # 满足不碰撞约束
        loop1 = loop()
        loop2 = loop()
        p1 = generate_theta(mul) * a + current_solution.dot1.theta
        p2 = generate_theta(mul) * a + current_solution.dot2.theta
        loop1.get(0, p1, state=2)
        loop2.get(0, p2, state=2)
        com = combined(loop1, loop2)
    return com


# 接受概率函数
def acceptance_probability(current_energy, new_energy, temperature):
    if new_energy < current_energy:
        return 1.0
    else:
        return np.exp((current_energy - new_energy) / temperature)


# 模拟退火算法
def simulated_annealing(objective, initial_solution, initial_temperature, cooling_rate, min_temperature, epoch=1000):
    """
    initial_solution：初始解，由 combined 类的一个实例表示。
    initial_temperature：初始温度，设定为 1000。
    cooling_rate：冷却率，设定为 0.95，每次迭代后温度乘以此值。
    min_temperature：最小温度，设定为 1e-5，当温度低于此值时停止迭代。
    """
    current_solution = initial_solution
    current_energy = objective(current_solution)
    temperature = initial_temperature

    # while temperature > min_temperature:
    for i in range(epoch):
        # 通过随机扰动当前解中的参数（如角度 theta）来生成新的解。
        new_solution = get_new_solution(current_solution)
        # com.get函数，返回~
        new_energy = objective(new_solution)

        if acceptance_probability(current_energy, new_energy, temperature) > np.random.rand():
            current_solution = new_solution
            current_energy = new_energy

        temperature *= cooling_rate

    return current_solution, current_energy


def init_solution():
    p = 1.7  # 螺距，单位：米
    b = p / (2 * np.pi)  # b的值

    a = 0.1
    mul = 15 * np.pi * 2 / 1.7  # 螺距计算的系数
    loop1 = loop()
    loop2 = loop()
    p1 = generate(mul=mul)
    p2 = generate(mul=mul)
    loop1.get(p1[0], p1[1])
    loop2.get(p2[0], p2[1])
    com = combined(loop1, loop2)
    return com


if __name__ == "__main__":
    # 参数设置
    # initial_solution = com  # 初始解
    initial_solution = init_solution()
    initial_temperature = 1000  # 初始温度
    cooling_rate = 0.95  # 冷却率
    min_temperature = 1e-5  # 最小温度

    # 运行模拟退火算法
    best_solution, best_energy = simulated_annealing(
        objective_function, initial_solution, initial_temperature, cooling_rate, min_temperature
    )

    print("Best Solution:", best_solution.dot1.__dict__, best_solution.dot2.__dict__, sep="\n")
    print("Best Energy:", best_energy)
    print(best_solution.get())
    # if __name__ == "__main__":
    #     pass
